#include "Device/motor/Motor.h"

void Motor::Init()
{
    driver->DRV_Init();
    encoder->Init();
    current_sample->Init();
}

void Motor::Adjust()
{
    speedLoop.Target =  target_speed;
    speedLoop.Current = GetSpeed();
    //currentLoop.Target = 
    currentLoop.Current = GetCurrent();
    speedLoop.Adjust();
    currentLoop.Adjust();
}

void Motor::Acuate()
{
    driver->SetOut(speedLoop.Out);
}

void Motor::Update(float _target_speed)
{
    encoder->UpdateRaw();
    UpdateSpeed();
    target_speed = _target_speed;
}

float Motor::GetCurrent()
{
	return 0;
}

//单位为rpm
int32_t Motor::GetSpeed()
{
    return speed;
}
void Motor::UpdateSpeed()
{
	speed = ((float)(encoder->getRawEncoder() - 32767)/1024.0f/encoder->getPeriod())*60.0f;
}
